#include "stdafx.h"
#include "CollisionResolution.h"

#include "CollisionData.h"
#include "RigidBody.h"

#include "MyMath.h"

#include <algorithm>

#define CT mphys::Contact

static XMVECTOR getBodyPointVelocity( mphys::Contact & contact, size_t bodyIndex, float dt )
{
	mphys::RigidBody & body = *contact.Body[bodyIndex];

	// Work out the velocity of the contact point.
	XMVECTOR velocity = XMVector3Cross( XMLoadFloat3( &body.AngularVelocity ), XMLoadFloat3( &contact.RelativeContactPosition[bodyIndex] ) );
	velocity = XMVectorAdd( velocity, XMLoadFloat3( &body.Velocity ) );

	// Turn the velocity into contact-coordinates.
	velocity = XMVector3TransformNormal( velocity, XMLoadFloat3x3( &contact.WorldToContact ) );

	//// Calculate the amount of velocity that is due to forces without
	//// reactions.
	//Vector3 accVelocity = thisBody->getLastFrameAcceleration() * duration;

	//// Calculate the velocity in contact-coordinates.
	//accVelocity = contactToWorld.transformTranspose(accVelocity);

	//// We ignore any component of acceleration in the contact normal
	//// direction, we are only interested in planar acceleration
	//accVelocity.x = 0;

	//// Add the planar velocities - if there's enough friction they will
	//// be removed during velocity resolution
	//contactVelocity += accVelocity;

	return velocity;
}

static void calculateDeltaVelocity( mphys::Contact & contact, float dt )
{
	const float velocityLimit = ( float ) 0.25f;

	XMVECTOR contactNormal = XMLoadFloat3( &contact.Normal );

	//// Calculate the acceleration induced velocity accumulated this frame
	float lastFrameVelChange = 0.0f;

	//if (body[0]->getAwake())
	//{
	lastFrameVelChange += XMVectorGetX( XMVector3Dot(
		XMLoadFloat3( &contact.Body[0]->LastFrameVelocityChange ),
		contactNormal ) );
	//}

	//if (body[1] && body[1]->getAwake())
	//{
	lastFrameVelChange -= XMVectorGetX( XMVector3Dot(
		XMLoadFloat3( &contact.Body[1]->LastFrameVelocityChange ),
		contactNormal ) );
	//	velocityFromAcc -=
	//		body[1]->getLastFrameAcceleration() * duration * contactNormal;
	//}


	float normalVelocity = contact.Velocity.x;

	// If the velocity is very slow, limit the restitution
	float restitution = contact.Restitution;
	if ( fabsf( normalVelocity ) < velocityLimit )
	{
		restitution = 0.0f;
	}

	// Combine the bounce velocity with the removed
	// acceleration velocity.
	contact.DeltaVelocity = -normalVelocity - restitution * ( normalVelocity - lastFrameVelChange ); // This line is rather correct, however complicated
}

static void calculateContactParams( mphys::Contact & contact, float dt )
{
	XMMATRIX contactToWorld = mmath::constructBaseFromVector( XMLoadFloat3( &contact.Normal ) );
	XMStoreFloat3x3( &contact.ContactToWorld, contactToWorld );
	XMStoreFloat3x3( &contact.WorldToContact, XMMatrixTranspose( contactToWorld ) );

	XMVECTOR contactPoint = XMLoadFloat3( &contact.Point );

	// Store the relative position of the contact relative to each body
	XMStoreFloat3( &contact.RelativeContactPosition[0],
		XMVectorSubtract( contactPoint, XMLoadFloat3( &contact.Body[0]->Position ) ) );
	XMStoreFloat3( &contact.RelativeContactPosition[1],
		XMVectorSubtract( contactPoint, XMLoadFloat3( &contact.Body[1]->Position ) ) );

	// Find the relative velocity of the bodies at the contact point.
	XMVECTOR velocity = XMVectorSubtract( getBodyPointVelocity( contact, 0, dt ), getBodyPointVelocity( contact, 1, dt ) );
	XMStoreFloat3( &contact.Velocity, velocity );

	// Calculate the desired change in velocity for resolution
	calculateDeltaVelocity( contact, dt );
}

static void resolveInterpenetrationFinal( mphys::Contact & contact, float interpenetration, XMVECTOR linearChange[2], XMVECTOR angularChange[2] )
{

	float angularMove[2];
	float linearMove[2];

	float totalInertia = 0;
	float linearInertia[2];
	float angularInertia[2];

	XMVECTOR contactNormal = XMLoadFloat3( &contact.Normal );
	XMVECTOR contactPoint = XMLoadFloat3( &contact.Point );

	XMVECTOR relativeContactPosition[2] = {
		XMLoadFloat3( &contact.RelativeContactPosition[0] ),
		XMLoadFloat3( &contact.RelativeContactPosition[1] )
	};

	// We need to work out the inertia of each object in the direction
	// of the contact normal, due to angular inertia only.
	for ( size_t i = 0; i < 2; i++ )
	{
		XMMATRIX wsInertiaTensorInverse = XMLoadFloat3x3( &contact.Body[i]->WorldSpaceInertiaTensorInverse );

		// Use the same procedure as for calculating frictionless
		// velocity change to work out the angular inertia.
		XMVECTOR angularInertiaWorld = XMVector3Cross( relativeContactPosition[i], contactNormal );
		angularInertiaWorld = XMVector3TransformNormal( angularInertiaWorld, wsInertiaTensorInverse );
		angularInertiaWorld = XMVector3Cross( angularInertiaWorld, relativeContactPosition[i] );
		angularInertia[i] = contact.Body[i]->MassInverse > 0.0f
			? XMVectorGetX( XMVector3Dot( angularInertiaWorld, contactNormal ) )
			: 0.0f;

		// The linear component is simply the inverse mass
		linearInertia[i] = contact.Body[i]->MassInverse;

		// Keep track of the total inertia from all components
		totalInertia += linearInertia[i] + angularInertia[i];

		// We break the loop here so that the totalInertia value is
		// completely calculated (by both iterations) before
		// continuing.
	}

	// Loop through again calculating and applying the changes
	for ( size_t i = 0; i < 2; i++ )
	{
		// The linear and angular movements required are in proportion to
		// the two inverse inertias.
		float sign = (i == 0) ? 1.0f : -1.0f;
		angularMove[i] = sign * interpenetration * ( angularInertia[i] / totalInertia );
		linearMove[i] = sign * interpenetration * ( linearInertia[i] / totalInertia );

		// To avoid angular projections that are too great (when mass is large
		// but inertia tensor is small) limit the angular move.
		XMVECTOR projection = XMVectorAdd(
			relativeContactPosition[i],
			XMVectorMultiply(
				contactNormal,
				XMVectorNegate( XMVector3Dot( relativeContactPosition[i], contactNormal ) )
			)
		);

		// Use the small angle approximation for the sine of the angle (i.e.
		// the magnitude would be sine(angularLimit) * projection.magnitude
		// but we approximate sine(angularLimit) to angularLimit).
		float maxMagnitude = 0.2f * XMVectorGetX( XMVector3LengthEst( projection ) );

		if (angularMove[i] < -maxMagnitude)
		{
			float totalMove = angularMove[i] + linearMove[i];
			angularMove[i] = -maxMagnitude;
			linearMove[i] = totalMove - angularMove[i];
		}
		else if (angularMove[i] > maxMagnitude)
		{
			float totalMove = angularMove[i] + linearMove[i];
			angularMove[i] = maxMagnitude;
			linearMove[i] = totalMove - angularMove[i];
		}

		// We have the linear amount of movement required by turning
		// the rigid body (in angularMove[i]). We now need to
		// calculate the desired rotation to achieve that.
		if (angularMove[i] == 0)
		{
			// Easy case - no angular movement means no rotation.
			angularChange[i] = XMVectorZero();
		}
		else
		{
			// Work out the direction we'd like to rotate in.
			XMVECTOR targetAngularDirection = XMVector3Cross( relativeContactPosition[i], contactNormal );

			XMMATRIX wsInertiaTensorInverse = XMLoadFloat3x3( &contact.Body[i]->WorldSpaceInertiaTensorInverse );

			// Work out the direction we'd need to rotate to achieve that
			angularChange[i] = contact.Body[i]->MassInverse > 0.0f
				? XMVectorMultiply(
					XMVector3TransformNormal( targetAngularDirection, wsInertiaTensorInverse ),
					XMVectorReplicate( angularMove[i] / angularInertia[i] ) )
				: XMVectorZero();
		}

		// Velocity change is easier - it is just the linear movement
		// along the contact normal.
		linearChange[i] = contact.Body[i]->MassInverse > 0.0f
			? XMVectorMultiply( contactNormal, XMVectorReplicate( linearMove[i] ) )
			: XMVectorZero();

		// Now we can start to apply the values we have calculated.
		// Apply the linear movement
		
		XMVECTOR position = XMLoadFloat3( &contact.Body[i]->Position );
		XMVECTOR orientation = XMLoadFloat4( &contact.Body[i]->Orientation );

		position = XMVectorAdd( position, linearChange[i] );

		XMVECTOR orientationChange = XMVectorSetW(angularChange[i], 0.0f);
		orientationChange = mmath::quaternionMultiply(orientationChange, orientation);
		orientation = XMVectorAdd(XMVectorMultiply(orientationChange, XMVectorReplicate(0.5f)), orientation);
		orientation = XMQuaternionNormalize(orientation); // fix inaccuracies

		XMStoreFloat3( &contact.Body[i]->Position, position );
		XMStoreFloat4( &contact.Body[i]->Orientation, orientation );
		
		// We need to calculate the derived data for any body that is
		// asleep, so that the changes are reflected in the object's
		// data. Otherwise the resolution will not change the position
		// of the object, and the next collision detection round will
		// have the same penetration.
		//if (!body[i]->getAwake()) body[i]->calculateDerivedData();
	}
}

static void updatePenetration( mphys::Contact & consideredContact, mphys::Contact & resolvedContact,
	size_t consideredContactBodyIndex, size_t resolvedContactBodyIndex,
	XMVECTOR linearPosChange[2], XMVECTOR angularPosChange[2] )
{
	size_t c = consideredContactBodyIndex;
	size_t r = resolvedContactBodyIndex;

	if (consideredContact.Body[c] == resolvedContact.Body[r])
	{
		// deltaPosition = linearPosChange + cross(angularPosChange, otherBodyRelativeContactPosition)
		XMVECTOR deltaPosition = XMVectorAdd(
			linearPosChange[r],
			XMVector3Cross(
			angularPosChange[r], 
			XMLoadFloat3( &consideredContact.RelativeContactPosition[c] )
			)
			);

		// The sign of the change is positive if we're
		// dealing with the second body in a contact
		// and negative otherwise (because we're
		// subtracting the resolution)..
		XMVECTOR affectedContactNormal = XMLoadFloat3( &consideredContact.Normal );
		consideredContact.Penetration += (c ? 1 : -1) * XMVectorGetX( XMVector3Dot( deltaPosition, affectedContactNormal ) );
	}
}

static void updatePenetrations( std::vector<mphys::Contact, util::AlignedMemAlloc<mphys::Contact, 16> > & contacts, mphys::Contact & resolvedContact, XMVECTOR linearPosChange[2], XMVECTOR angularPosChange[2])
{
	size_t contactsSize = contacts.size();

	for (size_t i = 0; i < contactsSize; i++)
	{
		if(resolvedContact.Body[0]->MassInverse > 0)
		{
			updatePenetration( contacts[i], resolvedContact, 0, 0, linearPosChange, angularPosChange);
			updatePenetration( contacts[i], resolvedContact, 1, 0, linearPosChange, angularPosChange);
		}
		if(resolvedContact.Body[1]->MassInverse > 0)
		{
			updatePenetration( contacts[i], resolvedContact, 0, 1, linearPosChange, angularPosChange);
			updatePenetration( contacts[i], resolvedContact, 1, 1, linearPosChange, angularPosChange);
		}
		//updatePenetration( i, resolvedContact, 0, 0, linearPosChange, angularPosChange);
		//updatePenetration( i, resolvedContact, 0, 1, linearPosChange, angularPosChange);
		//updatePenetration( i, resolvedContact, 1, 0, linearPosChange, angularPosChange);
		//updatePenetration( i, resolvedContact, 1, 1, linearPosChange, angularPosChange);
		//updatePenetration( contacts[i], resolvedContact, 0, 1, linearPosChange, angularPosChange);

		//// Check each body in the contact
		//for (size_t b = 0; b < 2; b++)
		//{
		//	// Check for a match with each body in the newly
		//	// resolved contact
		//	for (size_t d = 0; d < 2; d++)
		//	{
		//		if (contacts[i].Body[b] == resolvedContact.Body[d])
		//		{
		//			// deltaPosition = linearPosChange + cross(angularPosChange, otherBodyRelativeContactPosition)
		//			XMVECTOR deltaPosition = XMVectorAdd(
		//				linearPosChange[d],
		//				XMVector3Cross(
		//					angularPosChange[d], 
		//					XMLoadFloat3( &contacts[i].RelativeContactPosition[b] )
		//				)
		//			);

		//			// The sign of the change is positive if we're
		//			// dealing with the second body in a contact
		//			// and negative otherwise (because we're
		//			// subtracting the resolution)..
		//			XMVECTOR affectedContactNormal = XMLoadFloat3( &contacts[i].Normal );
		//			contacts[i].Penetration += (b ? 1 : -1) * XMVectorGetX( XMVector3Dot( deltaPosition, affectedContactNormal ) );
		//		}
		//	}
		//}
	}
}

static void resolveVelocityFinal( mphys::Contact & contact, XMVECTOR velocityChange[2], XMVECTOR angVelocityChange[2] )
{
	float totalMassInverse = contact.Body[0]->MassInverse + contact.Body[1]->MassInverse;
	if (totalMassInverse <= 0) return;

	XMVECTOR contactNormal = XMLoadFloat3( &contact.Normal );
	XMVECTOR contactPoint = XMLoadFloat3( &contact.Point );

	XMVECTOR vel0 = XMLoadFloat3( &contact.Body[0]->Velocity );
	XMVECTOR vel1 = XMLoadFloat3( &contact.Body[1]->Velocity );

	XMVECTOR angVel0 = XMLoadFloat3( &contact.Body[0]->AngularVelocity );
	XMVECTOR angVel1 = XMLoadFloat3( &contact.Body[1]->AngularVelocity );

	// change of velocity for entire collision (for both bodies) per unit impulse
	float deltaVelPerUnitImpulse = 0.0f;

	XMVECTOR relativeContactPosition[2] = {
		XMLoadFloat3( &contact.RelativeContactPosition[0] ),
		XMLoadFloat3( &contact.RelativeContactPosition[1] )
	};

	// following is calculation of deltaVelPerUnitImpulse as a sum of angular and linear velocity change per unit impulse for both objects

	XMMATRIX worldSpaceInertiaTensorInverse0 = XMLoadFloat3x3( &contact.Body[0]->WorldSpaceInertiaTensorInverse );

	// torque for entire body, created by unit impulse in contact point and in direction of contact normal
	XMVECTOR torquePerUnitImpulse = XMVector3Cross( relativeContactPosition[0], contactNormal );
	// angular velocity change (still for entire body)
	XMVECTOR rotationPerUnitImpulse = XMVector3TransformNormal( torquePerUnitImpulse, worldSpaceInertiaTensorInverse0 );
	// angular velocity change for contact point
	XMVECTOR velocityPerUnitImpulse = XMVector3Cross( rotationPerUnitImpulse, relativeContactPosition[0] );
	// component of angular velocity change for contact point along contact normal
	deltaVelPerUnitImpulse += contact.Body[0]->MassInverse > 0 ?
		XMVectorGetX( XMVector3Dot( velocityPerUnitImpulse, contactNormal ) )
		: 0.0f;
	// linear velocity change, created by unit impulse
	deltaVelPerUnitImpulse += contact.Body[0]->MassInverse;

	// the same for second body
	XMMATRIX worldSpaceInertiaTensorInverse1 = XMLoadFloat3x3( &contact.Body[1]->WorldSpaceInertiaTensorInverse );

	torquePerUnitImpulse = XMVector3Cross( relativeContactPosition[1], contactNormal );
	rotationPerUnitImpulse = XMVector3TransformNormal( torquePerUnitImpulse,  worldSpaceInertiaTensorInverse1);
	velocityPerUnitImpulse = XMVector3Cross( rotationPerUnitImpulse, relativeContactPosition[1] );
	deltaVelPerUnitImpulse += contact.Body[1]->MassInverse > 0 ?
		XMVectorGetX( XMVector3Dot( velocityPerUnitImpulse, contactNormal ) )
		: 0.0f;
	deltaVelPerUnitImpulse += contact.Body[1]->MassInverse;

	// end of deltaVelPerUnitImpulse calculation

	XMVECTOR impulse = XMVectorSet( contact.DeltaVelocity / deltaVelPerUnitImpulse, 0.0f, 0.0f, 0.0f );

	XMMATRIX contactToWorld = XMLoadFloat3x3( &contact.ContactToWorld );
							// = mmath::constructBaseFromVector( contactNormal );
	impulse = XMVector3TransformNormal( impulse, contactToWorld );

	velocityChange[0] = XMVectorMultiply( impulse, XMVectorReplicate( contact.Body[0]->MassInverse ) );
	velocityChange[1] = XMVectorMultiply( impulse, XMVectorReplicate( -contact.Body[1]->MassInverse ) );

	vel0 = XMVectorAdd( vel0, velocityChange[0] );
	vel1 = XMVectorAdd( vel1, velocityChange[1] );

	XMStoreFloat3( &contact.Body[0]->Velocity, vel0 );
	XMStoreFloat3( &contact.Body[1]->Velocity, vel1 );

	XMVECTOR impulseTorque = XMVector3Cross( relativeContactPosition[0], impulse );
	angVelocityChange[0] = contact.Body[0]->MassInverse > 0 ?
		XMVector3TransformNormal( impulseTorque, worldSpaceInertiaTensorInverse0 ) : XMVectorZero();
	angVel0 = XMVectorAdd( angVel0, angVelocityChange[0] );

	impulseTorque = XMVector3Cross( impulse, relativeContactPosition[1] );
	angVelocityChange[1] = contact.Body[1]->MassInverse > 0 ?
		XMVector3TransformNormal( impulseTorque, worldSpaceInertiaTensorInverse1 ) : XMVectorZero();
	angVel1 = XMVectorAdd( angVel1, angVelocityChange[1] );

	XMStoreFloat3( &contact.Body[0]->AngularVelocity, angVel0 );
	XMStoreFloat3( &contact.Body[1]->AngularVelocity, angVel1 );
}

static void updateVelocity( mphys::Contact & consideredContact, mphys::Contact & resolvedContact,
	size_t consideredContactBodyIndex, size_t resolvedContactBodyIndex,
	XMVECTOR velocityChange[2], XMVECTOR angVelocityChange[2], float dt )
{
	size_t c = consideredContactBodyIndex;
	size_t r = resolvedContactBodyIndex;

	if (consideredContact.Body[c] == resolvedContact.Body[r])
	{
		XMVECTOR deltaVel = XMVectorAdd( velocityChange[r],
			XMVector3Cross( angVelocityChange[r], XMLoadFloat3( &consideredContact.RelativeContactPosition[c] ) ) );

		// The sign of the change is negative if we're dealing
		// with the second body in a contact.
		XMVECTOR contactVel = XMLoadFloat3( &consideredContact.Velocity );
		XMMATRIX worldToContact = XMLoadFloat3x3( &consideredContact.WorldToContact );
		contactVel = XMVectorAdd(
			contactVel,
			XMVectorMultiply(
			XMVector3TransformNormal( deltaVel, worldToContact ),
			XMVectorReplicate( c ? -1.0f : 1.0f )
			)
			);
		XMStoreFloat3( &consideredContact.Velocity, contactVel );
		calculateDeltaVelocity( consideredContact, dt );
	}
}

static void updateVelocities( std::vector<mphys::Contact, util::AlignedMemAlloc<mphys::Contact, 16> > & contacts, mphys::Contact & resolvedContact,
	XMVECTOR velocityChange[2], XMVECTOR angVelocityChange[2], float dt )
{
	size_t contactsSize = contacts.size();

	// With the change in velocity of the two bodies, the update of
	// contact velocities means that some of the relative closing
	// velocities need recomputing.
	for (size_t i = 0; i < contactsSize ; i++)
	{
		//if (&contacts[i] == &resolvedContact)
		//	continue;
		//	
		if(resolvedContact.Body[0]->MassInverse > 0)
		{
			updateVelocity( contacts[i], resolvedContact, 0, 0, velocityChange, angVelocityChange, dt );
			updateVelocity( contacts[i], resolvedContact, 1, 0, velocityChange, angVelocityChange, dt );
		}
		if(resolvedContact.Body[1]->MassInverse > 0)
		{
			updateVelocity( contacts[i], resolvedContact, 0, 1, velocityChange, angVelocityChange, dt );
			updateVelocity( contacts[i], resolvedContact, 1, 1, velocityChange, angVelocityChange, dt );
		}
		
		//ble += updateVelocity( i, resolvedContact, 0, 1, velocityChange, angVelocityChange, dt );
		
		//ble += updateVelocity( i, resolvedContact, 1, 1, velocityChange, angVelocityChange, dt );

		//// Check each body in the contact
		//for (size_t b = 0; b < 2; b++)
		//{
		//	// Check for a match with each body in the newly
		//	// resolved contact
		//	for (size_t d = 0; d < 2; d++)
		//	{
		//		if (contacts[i].Body[b] == resolvedContact.Body[d])
		//		{
		//			XMVECTOR deltaVel = XMVectorAdd( velocityChange[d],
		//				XMVector3Cross( angVelocityChange[d], XMLoadFloat3( &contacts[i].Derived.RelativeContactPosition[b] ) ) );

		//			// The sign of the change is negative if we're dealing
		//			// with the second body in a contact.
		//			XMVECTOR contactVel = XMLoadFloat3( &contacts[i].Derived.Velocity );
		//			XMMATRIX worldToContact = XMLoadFloat3x3( &contacts[i].Derived.WorldToContact );
		//			contactVel = XMVectorAdd(
		//				contactVel,
		//				XMVectorMultiply(
		//					XMVector3TransformNormal( deltaVel, worldToContact ),
		//					XMVectorReplicate( b ? -1.0f : 1.0f )
		//				)
		//			);
		//			XMStoreFloat3( &contacts[i].Derived.Velocity, contactVel );
		//			calculateDeltaVelocity( contacts[i], dt );
		//		}
		//	}
		//}
	}
	
}

void mphys::resolveContactsComplete( CollisionData & data, float dt )
{
	//LOG_STREAM("sizeof contacts " << sizeof(Contact));
	//LOG_STREAM("offset of Point " << offsetof(Contact, Point));
	//LOG_STREAM("offset of Penetration " << offsetof(Contact, Penetration));
	//LOG_STREAM("offset of ContactToWorld " << offsetof(Contact, ContactToWorld));
	//LOG_STREAM("offset of Normal " << offsetof(Contact, Normal));
	//LOG_STREAM("offset of WorldToContact " << offsetof(Contact, WorldToContact));
	//LOG_STREAM("offset of Velocity " << offsetof(Contact, Velocity));
	//LOG_STREAM("offset of DeltaVelocity " << offsetof(Contact, DeltaVelocity));
	//LOG_STREAM("offset of RelativeContactPosition " << offsetof(Contact, RelativeContactPosition));

	std::vector<mphys::Contact, util::AlignedMemAlloc<mphys::Contact, 16> > & contacts = data.getContacts();
	//auto contacts = data.getContacts();
	size_t contactsSize = contacts.size();

	const size_t MAX_ITERATIONS = 2 * contactsSize;

	for ( size_t i = 0; i < contactsSize ; i++ )
	{
		calculateContactParams( contacts[i], dt );
	}

	if( contactsSize == 0 )
		return;

	XMVECTOR linearPosChange[2], angularPosChange[2];

	for (size_t iter = 0; iter < MAX_ITERATIONS; iter++)
	{
		size_t nextContact = 0;
		float maxPenetration = std::numeric_limits<float>::lowest();

		for (size_t i = 0; i < contactsSize ; i++)
		{
			float penetration = contacts[i].Penetration;
			if(penetration > maxPenetration)
			{
				maxPenetration = penetration;
				nextContact = i;
			}
		}

		// All contacts moved away. Finish.
		if(maxPenetration <= 0.001f)
			break;

		resolveInterpenetrationFinal(contacts[nextContact], maxPenetration, linearPosChange, angularPosChange);

		updatePenetrations(contacts, contacts[nextContact], linearPosChange, angularPosChange);
	}

	XMVECTOR velocityChange[2], angVelocityChange[2];

	for ( size_t iter = 0; iter < MAX_ITERATIONS; iter++ )
	{
		size_t nextContact = -1;
		float maxDeltaVelocity = std::numeric_limits<float>::epsilon();
		// minimum separating velocity means maximum closing velocity = most serious contact
		for ( size_t i = 0; i < contactsSize ; i++ )
		{
			if( contacts[i].DeltaVelocity > maxDeltaVelocity )
			{
				maxDeltaVelocity = contacts[i].DeltaVelocity;
				nextContact = i;
			}
		}

		// All contacts are separating. Finish.
		if( nextContact == -1 )
			break;

		//resolveContact(contacts[nextContact], dt);
		resolveVelocityFinal( contacts[nextContact], velocityChange, angVelocityChange );

		updateVelocities( contacts, contacts[nextContact], velocityChange, angVelocityChange, dt );

		// Doesn't make sense to resolve penetration here (with order for velocity) - horrible jitter!
		// If sorting for penetration resolution is too slow, even simple penetration resolution
		// of all contacts sequentially is better than this!
		//resolveInterpenetration(contacts[nextContact], contacts[nextContact].Penetration);
	}





	
}
